﻿using System.Runtime.InteropServices;

namespace PLMonitor;

/// <summary>
/// 上位机发送的数据包结构体
/// </summary>
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
public struct Gcu2GbcPkt_t
{
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
    public byte[] sync; // 注释 1 0xA9 0x5B
    //struct
    //{
    //    byte trig:3; // 注释 2 命令触发计数
    //    byte value:5; // 注释 3 0-未定义；1-陀螺仪校准；2-启动云台；3-停止云台；4-手动控制；5-指点 平移
    //} cmd;
    public byte cmd; // 
    //struct
    //{
    //    byte:3;
    //    byte fl_sens:5; // 注释 4
    //} aux;
    public byte aux;

    //struct
    //{
    //    byte:3;
    //    byte go_zero:1; // 注释 5
    //    byte wk_mode:2; // 注释 6
    //    byte op_type:2; // 注释 7
    //    short op_value; // 注释 8
    //} gbc[3]; // 注释 9
    public byte gbc0Flags;
    public short gbc0Val;

    public byte gbc1Flags;
    public short gbc1Val;

    public byte gbc2Flags;
    public short gbc2Val;
    //struct
    //{
    //    byte:7;
    //    byte valid:1; // 注释 10
    //    short angle[3]; // 注释 11
    //    short accel[3]; // 注释 12
    //} uav; // 注释 13
    public byte  uavFlags;
    public short uavAngX;
    public short uavAngY;
    public short uavAngZ;
    public short uavAccX;
    public short uavAccY;
    public short uavAccZ;

    //struct
    //{
    //    uint vert_fov1x:7; // 注释 14
    //    uint zoom_value:24; // 注释 15
    //    uint reserved:1; // 注释 16
    //    float target_angle[2]; // 注释 17
    //}  cam;
    public uint camFlags;
    public float targetAngX;
    public float targetAngY;

    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
    public byte[] crc; // 注释 18
}

/// <summary>
/// 云台返回的数据包结构体
/// </summary>
[StructLayout(LayoutKind.Sequential, CharSet = CharSet.Ansi, Pack = 1)]
public struct Gbc2GcuPkt_t
{
    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
    public byte[] sync; // 注释 19
    public byte fw_ver; // 注释 20
    public byte hw_err; // 注释 21

    //byte inv_flag:1; // 注释 22
    //byte gbc_stat:3; // 注释 23
    //byte tca_flag:1; // 注释 24
    //byte:3;
    public byte flags;

    //struct
    //{
    //    byte stat:3; // 注释 25
    //    byte value:5; // 注释 26
    //} cmd;
    public byte cmd;

    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public short[] cam_rate; // 注释 27

    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public short[] cam_angle; // 注释 28

    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 3)]
    public short[] mtr_angle; // 注释 29

    [MarshalAs(UnmanagedType.ByValArray, SizeConst = 2)]
    public byte[] crc; // 注释 30
}

// PTZ状态数据
public class PTZInfo
{
    public byte fw_ver; // 注释 20
    public byte hw_err; // 注释 21
    public byte flags;
    public byte cmd;
    public float[] CamRate = new float[3];
    public float[] CamAngle = new float[3];
    public float[] MtrAngle = new float[3];
}

// PTZ控制命令
internal class PTZControl
{
    #region 成员变量
    public PTZInfo Info = new();
    private readonly byte[] _rxBuf = new byte[128];
    private int _rxCnt = 0;
    private bool _isNewPacket = false;
    #endregion

    public PTZControl() 
    {
        //var testData = new byte[] {
        //    0xA9,0x5B,0x20,0x00,0x90,0x00,0x00,0x90,
        //    0x0A,0x00,0x90,0x00,0x00,0x80,0xF4,0x01,
        //    0xF4,0x01,0xF4,0x01,0x0A,0x00,0x0A,0x00,
        //    0x0A,0x00,0x00,0x00,0x00,0x00,0x00,0x00,
        //    0x00,0x00,0x00,0x00,0x00,0x00,0x98,0xDB
        //};
        
        //var ret = DataUtils.BytesToStruct<Gcu2GbcPkt_t>(testData);

        //var len = testData.Length;
        //var crc = CalcCrc16(testData, len - 2);
        //if (crc == testData[len - 2] * 256 + testData[len - 1])
        //{
        //    Console.WriteLine("OK");
        //}

        // ProcRcvData(testData, testData.Length);
    }

    public bool ProcRcvData(byte[] data, int count)
    {
        _isNewPacket = false;
        for (int i = 0; i < count; i++)
        {
            PutChar(data[i]);
        }

        return _isNewPacket;
    }

    /// <summary>
    /// 基于字节流协议解析
    /// </summary>
    private void PutChar(byte c)
    {
        // 协议头 0xB5 0x9A
        if (_rxCnt == 0 && c != 0xB5 ||
           _rxCnt == 1 && c != 0x9A)
        {
            _rxCnt = 0;
        }
        _rxBuf[_rxCnt++] = c;

        if (_rxCnt == Marshal.SizeOf<Gbc2GcuPkt_t>())
        {
            // 计算CRC值
            var crc = CalcCrc16(_rxBuf, _rxCnt - 2);
            if (crc == _rxBuf[_rxCnt - 2] * 256 + _rxBuf[_rxCnt - 1])
            {
                _isNewPacket = true;
                // 解析字段
                var index = 2;
                Info.fw_ver = _rxBuf[index++];
                Info.hw_err = _rxBuf[index++];
                Info.flags = _rxBuf[index++];
                Info.cmd = _rxBuf[index++];
                // 相机本体角速度 0.1deg/s
                Info.CamRate[0] = 0.1f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.CamRate[1] = 0.1f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.CamRate[2] = 0.1f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                // 相机姿态角， 单位0.01deg
                Info.CamAngle[0] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.CamAngle[1] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.CamAngle[2] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                // 相机相对角 ， 单位0.01deg
                Info.MtrAngle[0] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.MtrAngle[1] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                Info.MtrAngle[2] = 0.01f * DataUtils.GEN_WORD_S(_rxBuf[index + 1], _rxBuf[index]);
                index += 2;
                //
            }
            _rxCnt = 0;
        }

        // 接收数据溢出
        if (_rxCnt > (_rxBuf.Length - 2))
        {
            _rxCnt = 0;
        }
    }

    /// <summary>
    /// 向云台发送控制指令
    /// </summary>
    public void SendCtrlPacket()
    {
        var packet = new Gcu2GbcPkt_t
        {
            sync = [0xA9, 0x5B],
            cmd = 4 // 命令码 4：手动控制
        };

        //packet.cmd.trig = 0; // 无需重复触发手动控制命令，只需执行一次即可
        
        // 滚转轴：锁定模式 + 角度控制（0deg）
        //packet.gbc[0].wk_mode = 1; // 锁定模式
        //packet.gbc[0].op_type = 0; // 角度控制
        //packet.gbc[0].op_value = 0;

        // 俯仰轴：锁定模式 + 真实角速度控制（0deg/s）
        //packet.gbc[1].wk_mode = 1;
        //packet.gbc[1].op_type = 2;
        //packet.gbc[1].op_value = 0;

        // 偏航轴：跟随模式 + 真实角速度控制（0deg/s）
        //packet.gbc[2].wk_mode = 0;
        //packet.gbc[2].op_type = 2;
        //packet.gbc[2].op_value = 0;

        // 载机数据（模拟值）
        //packet.uav.valid = 0; // 载机数据无效。如无准确真实的载机姿态角和加速度，此数据务必置 0

        // 欧拉角转换（deg-> 0.01deg）
        //packet.uav.angle[0] = (int16_t)(simulated_ned_angle[0] * 100); // 滚转
        //packet.uav.angle[1] = (int16_t)(simulated_ned_angle[1] * 100); // 俯仰
        //packet.uav.angle[2] = (int16_t)(simulated_ned_angle[2] * 100); // 偏航
        //                                                               // 加速度转换（m/s2-> 0.01m/s2）
        //packet.uav.accel[0] = (int16_t)(simulated_ned_accel[0] * 100); // 北向
        //packet.uav.accel[1] = (int16_t)(simulated_ned_accel[1] * 100); // 东向
        //packet.uav.accel[2] = (int16_t)(simulated_ned_accel[2] * 100); // 天向
    }

    /// <summary>
    /// 计算CRC校验
    /// </summary>
    private static ushort CalcCrc16(byte[] ptr, int len)
    {
        ushort crc;
        byte da;
        ushort[] crc_ta = [ 0x0000,0x1021,0x2042,0x3063,0x4084,0x50a5,0x60c6,0x70e7,
                            0x8108,0x9129,0xa14a,0xb16b,0xc18c,0xd1ad,0xe1ce,0xf1ef, ];
        crc = 0;
        for (var i = 0; i < len; i++)
        {
            da = (byte)(crc >> 12);
            crc <<= 4;
            crc ^= crc_ta[da ^ (ptr[i] >> 4)];
            da = (byte)(crc >> 12);
            crc <<= 4;
            crc ^= crc_ta[da ^ (ptr[i] & 0x0F)];
        }
        return crc;
    }

}
